#include <crate_vision/main.h>
#include <crate_vision/CrateVisionNode.h>

#include <pcrctransformation/pcrctransformer.hpp>
#include <pcrctransformation/point2f.hpp>

VisionData visionData;

VisionData* getVisionData() {
	return &visionData;
}


void initVisionData();

int main(int argc, char* argv[]) {
	initVisionData();	

	new CrateVisionNode(argc, argv);

	return 0;
}


void initVisionData() {			
	visionData.crateData.width = 115;
	visionData.crateData.height = visionData.crateData.width;
	
	visionData.crateData.row_nr = 4;
	visionData.crateData.col_nr = 4;
	
	visionData.crateData.median = 11;
	
	visionData.crateData.container_width = visionData.crateData.width/visionData.crateData.col_nr;
	visionData.crateData.container_height = visionData.crateData.height/visionData.crateData.row_nr;
	
	visionData.contentData.diff_colors = 3;
	
	visionData.contentData.ball_hue_low = new int[visionData.contentData.diff_colors];
	visionData.contentData.ball_hue_high = new int[visionData.contentData.diff_colors];

			
	for(int i = 0; i < visionData.contentData.diff_colors; i++){
		visionData.contentData.ball_hue_low[i] = 180;
		visionData.contentData.ball_hue_high[i] = 0;
	}

	visionData.crateData.offset = 10;

	visionData.contentData.border_saturation = 0;
	visionData.contentData.empty_hue_low = 180;
	visionData.contentData.empty_hue_high = 0;
	
	point2f::point2fvector rc;
	
	/*rc.push_back(point2f(-59, -75));
	rc.push_back(point2f(-54, 113));
	rc.push_back(point2f(71, 110));*/
	
	rc.push_back(point2f(-65 + 1.5, -74));
	rc.push_back(point2f(-62.5 + 1.5, 113.5));
	rc.push_back(point2f(61.5 + 1.5, 110.5));
	
	//rc.push_back(point2f(100, 100));
	//rc.push_back(point2f(100, 0));
	//rc.push_back(point2f(0, 0);

	visionData.envData.cordTransformer = new pc_rc_transformer(rc,rc);
}

point2f single_perspective_adjustment(point2f src, bool real) {
	point2f result;
	double local_cam_height = visionData.envData.cam_rheight;
	double local_cam_x = visionData.envData.cam_rx;
	double local_cam_y = visionData.envData.cam_ry;
	double offset_x, offset_y, height, phi_x, phi_y;
	double local_crate_height = visionData.robotCrateData.depth;
	
	if(!real){
		std::cout << "not real" << std::endl;
		local_cam_height = visionData.envData.cam_pheight;
		local_crate_height = visionData.crateData.depth;
		local_cam_x = visionData.envData.cam_px;
		local_cam_y = visionData.envData.cam_py;
	}
	
	//std::cout << "src: " << src.x << " " << src.y << " cam pos: " << local_cam_x << " " << local_cam_y << " " << local_cam_height << std::endl;
	
	offset_x = src.x-local_cam_x;
	offset_y = src.y-local_cam_y;
	height = local_cam_height-local_crate_height;
	
	phi_x = atan(height/offset_x);
	phi_y = atan(height/offset_y);
	
	result.x = local_cam_height / tan(phi_x) + local_cam_x;

	result.y = local_cam_height / tan(phi_y) + local_cam_y;
	
	//std::cout << "result: " << result.x << " " << result.y << std::endl;	
	//std::cout << "phi_x: " << phi_x << " phi_y: " << phi_y << " offset_x: " << offset_x << " offset_y: " << offset_y << " height: " << height << std::endl;
	
	
	return result;
}

void draw_box(Mat& dst, int x1, int y1, int x2, int y2, Scalar color) {
	line(dst, Point(x1, y1), Point(x1, y2), color, 1, 8, 0);
	line(dst, Point(x1, y1), Point(x2, y1), color, 1, 8, 0);
	line(dst, Point(x2, y1), Point(x2, y2), color, 1, 8, 0);
	line(dst, Point(x1, y2), Point(x2, y2), color, 1, 8, 0);
}
